/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
          /*LINTLIBRARY*/
          /*PROTOLIB1*/

#include "main.h"
#include <math.h>
#include <stdio.h>
#include "delay.h"
#include "celp_sup.h"

static void Resample(
float	sig_in[],
float	wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC],
int	q_frac_pit,
int	out_len,
int	out_start,
int	int_pit,
int	m1,
int	m2,
float	sig_out[]);

static void GenHam(
int	nfrac,
float	frac[MAX_NFRAC],
int	m1,
int	m2,
int	twelfths[MAX_NFRAC],
float	wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC]);

static int quan_frac(
float	frac_pit,
int	nfrac,
float	frac[MAX_NFRAC]);

float sinc(
float	arg);

/*************************************************************************
*
* ROUTINE
*		delay
*
* FUNCTION
*		Time delay a bandlimited signal of either 8 or 40 points
*		using point-by-point recursion
*
* SYNOPSIS
*		delay(SigIn, SigInLen, SigOutStart, SigOutLen, FracPit, 
*			IntPit, IntLow, IntUp, NumFracDelay, SigOut)
*
*   formal 
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	SigIn		float	i/o	signal input (output in last 60)
*	SigInLen	int	 i	length of input sequence x(n)
*	SigOutStart	int	 i	beginning of output sequence
*	SigOutLen	int	 i	length of output sequence
*	FracPit		float	 i	fractional pitch
*	IntPit		int	 i	integer pitch
*	IntLow		int	 i	Lower interpolation bound
*	IntUp		int	 i	Upper interpolation bound
*	NumFracDelay	int	 i	Number of fractional delays
*	SigOut		float	 o	delayed input signal
*==========================================================================
*	
* DESCRIPTION
*
*	Subroutine to time delay a bandlimited signal by resampling the
*	reconstructed data (aka sinc interpolation).  The well known
*	reconstruction formula is:
*
*              |    M2      sin[pi(t-nT)/T]    M2
*   y(n) = X(t)| = SUM x(n) --------------- = SUM x(n) sinc[(t-nT)/T]
*              |   n=M1         pi(t-nT)/T    n=M1
*              |t=n+d
*
*	The interpolating (sinc) function is Hamming windowed to bandlimit
*	the signal to reduce aliasing.
*
*	Multiple simultaneous time delays may be efficiently calculated
*	by polyphase filtering.  Polyphase filters decimate the unused
*	filter coefficients.  See Chapter 6 in C&R for details. 
*==========================================================================
*	
* REFERENCES
*
*	Crochiere & Rabiner, Multirate Digital Signal Processing,
*	P-H 1983, Chapters 2 and 6.
*
*	Kroon & Atal, "Pitch Predictors with High Temporal Resolution,"
*	ICASSP '90, S12.6
*
***************************************************************************/

void delay(
float	SigIn[],
int	SigInLen,
int	SigOutStart,
int	SigOutLen,
float	FracPit,
int	IntPit,
int	IntLow,
int	IntUp,
int	NumFracDelay,
float	SigOut[])
{
static int	LongFirst=TRUE, ShortFirst=TRUE;
static float	Longwsinc[MAX_M1+MAX_M2+1][MAX_NFRAC];
static float	Shortwsinc[MAX_M1+MAX_M2+1][MAX_NFRAC];
int		twelfths[MAX_NFRAC] = {3,4,6,8,9};
float		frac[MAX_NFRAC] = {0.25, 0.3333333, 0.5, 0.66666667, 0.75};
int		q_frac_pit;
int		ldelay;

/*  Determine whether this is a long or short delay */
	if(IntLow == -20)
	  ldelay = TRUE;
	else
	  ldelay = FALSE;

/*  Generate appropriate Hamming windowed sinc interpolating functions */
	if (LongFirst)	{
	  if(ldelay)	{
	    LongFirst = FALSE;

/*  Generate Hamming windowed sinc interpolating function for each 
	allowable fraction */
	    GenHam(NumFracDelay, frac, IntLow, IntUp, twelfths, Longwsinc);
	  }
	}
	
	if (ShortFirst)	{
	  if(!ldelay)	{
	    ShortFirst = FALSE;

/*  Generate Hamming windowed sinc interpolating function for each 
	allowable fraction */
	    GenHam(NumFracDelay, frac, IntLow, IntUp, twelfths, Shortwsinc);
	  }
	}

/*  Quantize the fractional pitch */
	q_frac_pit = quan_frac(FracPit, NumFracDelay, frac);

/*  Resample */
	if(ldelay)
	  Resample(SigIn, Longwsinc, q_frac_pit, SigOutLen, SigOutStart, 
		IntPit, IntLow, IntUp, SigOut);
	else
	  Resample(SigIn, Shortwsinc, q_frac_pit, SigOutLen, SigOutStart, 
		IntPit, IntLow, IntUp, SigOut);


}
/*

************************************************************************
*
* ROUTINE
*		GenHam
*
* FUNCTION
*
*		Generate Hamming windowed sinc interpolating function
*
* SYNOPSIS
*		GenHam(nfrac, frac, m1, m2, twelfths, wsinc, hwin)
*
*   formal 
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	nfrac		int	 i	number of fractional delays
*	frac		float	 i	"nfrac" fractional delays calculated 
*					over interpolation of M1 to M2
*	m1		int	 i	Lower interpolation bound
*	m2		int	 i	Upper interpolation bound
*	twelfths	int	 i	???
*	wsinc		float	 o	Hamming windowed sinc interpolating
*					function
*	hwin		float	 o	Hamming window
**************************************************************************
*
*	Generate Hamming windowed sinc interpolating function
*	for each allowable fraction.  The interpolating functions
*	are stored in time reverse order (i.e., delay appears as
*	advance) to align with the adaptive code book v0 array.
*	The interp filters are:
*		wsinc(.,1)	frac = 1/4 (3/12)
*		wsinc(.,2)	frac = 1/3 (4/12)
*		.		.
*		wsinc(.,5)	frac = 3/4 (9/12)
*
**************************************************************************/

void GenHam(
int	nfrac,
float	frac[MAX_NFRAC],
int	m1,
int	m2,
int	twelfths[MAX_NFRAC],
float	wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC])
{
int	i,j;
float	hwin[MAX_WINDOW_WIDTH+1]; 

	CreateHam(hwin, 12*(m2-m1+1)+1);

	for(i=0; i<nfrac; i++)	{
	  for(j=m1; j<m2+1; j++)	{
	    wsinc[(j-m1)][i] = sinc((float)(frac[i] + j));
	    wsinc[(j-m1)][i] *= hwin[12 * (j-m1) + twelfths[i]];
	  }
	}

}
/*

************************************************************************
*
* ROUTINE
*		quan_frac
*
* FUNCTION
*
*  		Quantize the fractional pitch 
*
* SYNOPSIS
*		q_frac_pit = quan_frac(frac_pit, nfrac, frac);
*
*
*   formal 
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	frac_pit	float	 i	Fractional pitch
*	nfrac		int	 i	Number of fractional delays
*	frac		float	 i	"nfrac" fractional delays calculated 
*					over interpolation of m1 to m2
*
*	q_frac_pit	int	 o	Quantized fractional pitch
**************************************************************************/

int quan_frac(
float	frac_pit,
int	nfrac,
float	frac[MAX_NFRAC])
{
int 	ok=FALSE;
int	i, k;

	for(i=0;i<nfrac;i++)	{
	  if (fabs(frac_pit - frac[i]) < .01)	{
	    k = i;
	    ok = TRUE;
	  }
	}

	if (!ok)	{
/*  This should not occur.  The reason this code has been added is to
	compensate for faulty interpolation in error correction of
	corrupted fractional pitch indexes.
*/
	  k=2;  
	  printf("delay.c:  Invalid adaptive delay = %1.3f\n",frac_pit);
	}

	return k;

}
/*

************************************************************************
*
* ROUTINE
*		Resample
*
* FUNCTION
*
*  		Upsample and select appropriate samples to resample
*		at fractional delay 
*
* SYNOPSIS
*
*		Resample(sig_in, wsinc, q_frac_pit, out_len, 
*			out_start, int_pit, m1, m2, sig_out)
*
*
*
*   formal 
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	sig_in		float	 i	Input signal
*	wsinc		float	 i	Hamming windowed sinc interpolating
*					function
*	q_frac_pit	int	 i	Quantized fractional pitch
*	out_len		int	 i	Length of output signal
*	out_start	int	 i	Beginning of output signal
*	int_pit		int	 i	Integer pitch
*	m1		int	 i	Lower interpolation bound
*	m2		int	 i	Upper interpolation bound
*	sig_out		float	 o	Delayed input signal
**************************************************************************/

void Resample(
float	sig_in[],
float	wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC],
int	q_frac_pit,
int	out_len,
int	out_start,
int	int_pit,
int	m1,
int	m2,
float	sig_out[])
{
int	i,j;

	for(i=0; i<out_len; i++)	{
	  sig_in[out_start + i-1] = 0.0;
	  for (j=m1; j<m2+1; j++)	{
	    sig_in[out_start + i-1] += sig_in[out_start - int_pit + i + j -1] * wsinc[j-m1][q_frac_pit];
	  }
	}

	for(i=0; i<out_len; i++)	{
	  sig_out[i] = sig_in[out_start + i - 1];
	  sig_in[out_start+i-1] = 0.0;
	}

}
/*

************************************************************************
*
* ROUTINE
*		sinc
*
* FUNCTION
*
*  		Calculate sinc funtion of input 
*
* SYNOPSIS
*		sinc_arg = sinc(arg);
*
*
*   formal 
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	arg		float	 i	Argument
*
*	sinc_arg	float	 o	sin(pi*arg) / pi*arg
**************************************************************************/

float sinc(
float	arg)
{
float 	sinc_arg;
float	pi;

	pi = 4.0 * atan(1.0);

	if(arg == 0.0)
	  sinc_arg = 1.0;
	else
	  sinc_arg = (float)(sin((double)(pi * arg)) / (pi * arg));

	return sinc_arg;

}
